/***************************************************************** 
 *
 * FILE        : System_inits.c
 * 
 * DESCRIPTION : This file includes the system initialitations for
 *               MPC5604B
 *	             
 *               
 * 
 * COPYRIGHT   :(c) 2009, Freescale & STMicroelectronics 
 * 
 * VERSION     : 01.00 
 * DATE        : 26 SEPTEMBER 2012 
 * AUTHOR      : UTCH RAPTOR TEAM
 * HISTORY     :  
 *               
 *
 ******************************************************************/ 
 
#include "MPC5604B_M27V.h"

 void initModesAndClock(void) {
  ME.MER.R = 0x0000001D;          /* Enable DRUN, RUN0, SAFE, RESET modes */
                                  /* Initialize PLL before turning it on: */
  CGM.FMPLL_CR.R = 0x02400100;    /* 8 MHz xtal: Set PLL0 to 64 MHz */   
  ME.RUN[0].R = 0x001F0074;       /* RUN0 cfg: 16MHzIRCON,OSC0ON,PLL0ON,syclk=PLL0 */
  ME.RUNPC[1].R = 0x00000010; 	  /* Peri. Cfg. 1 settings: only run in RUN0 mode */
  ME.PCTL[32].R = 0x01; 	      /* MPC56xxB/P/S ADC 0: select ME.RUNPC[1] */	
  ME.PCTL[68].R = 0x01; 	      /* MPC56xxB/S SIU: select ME.RUNPC[1] */	
  ME.PCTL[72].R = 0x01;           /* MPC56xxB/S EMIOS 0:  select ME.RUNPC[0] */	
  ME.PCTL[57].R = 0x01;                                 /* Mode Transition to enter RUN0 mode: */
  ME.MCTL.R = 0x40005AF0;         /* Enter RUN0 Mode & Key */
  ME.MCTL.R = 0x4000A50F;         /* Enter RUN0 Mode & Inverted Key */  
  while (ME.GS.B.S_MTRANS) {}     /* Wait for mode transition to complete */    
                                  /* Note: could wait here using timer and/or I_TC IRQ */
  while(ME.GS.B.S_CURRENTMODE != 4) {} /* Verify RUN0 is the current mode */
}

void disableWatchdog(void) {
  SWT.SR.R = 0x0000c520;     /* Write keys to clear soft lock bit */
  SWT.SR.R = 0x0000d928; 
  SWT.CR.R = 0x8000010A;     /* Clear watchdog enable (WEN) */
}   


void initPeriClkGen(void) {
/* Use the following code as required for MPC56xxB or MPC56xxS:*/
  CGM.SC_DC[2].R = 0x80;   /* MPC56xxB/S: Enable peri set 3 sysclk divided by 1 */
}

void port_init (void) {
SIU.PCR[16].B.OBE= 1; /*Define PB[0](pad[16]) as output for enable the left motor
                        "EN_MOTORL"*/
                        
SIU.PCR[17].B.OBE= 1; /*Define PB[1](pad[17]) as output for enable the right motor
                        "EN_MOTORD"*/
                        
SIU.PCR[27].B.OBE= 1; /*Define PB[11](pad[27]) as output to the start signal for 
                        the camera*/
                        
SIU.PCR[29].B.OBE= 1; /*Define PB[13](pad[29]) as output to the clock signal for
                        the camera*/
                        
SIU.PCR[64].B.IBE = 1;/*Define PE[0](pad[64]) as input for the push botton 1
                        "PUSH1"*/
                        
SIU.PCR[65].B.IBE = 1;/*Define PE[1](pad[65]) as input for the push botton 2
                        "PUSH2"*/
                        
SIU.PCR[66].B.IBE = 1;/*Define PE[2](pad[66]) as input for the push botton 3
                        "PUSH3"*/
                        
SIU.PCR[67].B.IBE = 1;/*Define PE[3](pad[67]) as input for the push botton 4
                        "PUSH4"*/
                        
SIU.PCR[68].B.OBE= 1; /*Define PE[0](pad[68]) as output for the led 1 "LED1'
                       */
                        
SIU.PCR[69].B.OBE= 1; /*Define PE[1](pad[69]) as output for the led 2 "LED2'
                       */
                        
SIU.PCR[70].B.OBE= 1; /*Define PE[2](pad[70]) as output for enable the led 3 "LED3'
                       */
                        
SIU.PCR[71].B.OBE= 1; /*Define PE[3](pad[71]) as output for enable the led 4 "LED4'
                       */
}

void System_inits_all(void)
{
  initModesAndClock();    
  disableWatchdog();      
  initPeriClkGen();       
  port_init ();	
}